#include "ImageGradient.h"


HOG::ImageGradient::ImageGradient(int _nbins, bool _gammaCorrection):nbins(_nbins),gammaCorrection(_gammaCorrection)
{
}
HOG::ImageGradient::~ImageGradient() 
{
	
}
// 计算图像的梯度数据 结果存在grad及qangle中
// grad 与 qangle 都是2维矩阵 
// grad[][0] 表示的是当前这个梯度在最临近的两个角度方位内低角度的值
// grad[][1] 表示的是当前这个梯度在最临近的两个角度方位内高角度的值
// qangle[][0] 表示的低角度的索引
// qangle[][1] 表示的高角度的索引
bool HOG::ImageGradient::CalculateGradient(IplImage * Image, cv::Mat& grad, cv::Mat& qangle)
{
  // 判断图片是否是单通道的灰度图或者是3通道的图像
  CV_Assert( Image->nChannels == 1 || Image->nChannels == 3 );
  
  // 扩展图片 使得边界能够进行[-1,0,1]运算
  imageSize = cvSize(Image->width,Image->height);
  
  
  grad.create(imageSize,CV_32FC2) ; // <magnitude*(1-alpha),magnitude*alpha>
  qangle.create(imageSize,CV_8UC2) ; // quantized gradient orientation

  int cn = Image->nChannels ;


  // gamma 矫正
  double lut[256] ;
  if( gammaCorrection )
    for( int i = 0 ; i < 256 ; ++ i )
      lut[i] = std::sqrt((float)i);
  else
    for( int i = 0 ; i < 256 ; ++ i )
      lut[i] = (float)i ;

  // 开辟空间xmap 和 ymap 用来进行坐标映射， 从而来使得边界扩充
  // 其中 imageSize.width + 2 分配给 xmap 
  // imageSize.height +2 分配给ymap 
  // +2 是因为在进行[-1,0,1]的时候需要额外的两个像素
  unsigned * mapbuff = new unsigned[imageSize.width+imageSize.height+4] ;
  unsigned * xmap = mapbuff+1 ;
  unsigned * ymap = mapbuff+imageSize.width + 3 ;
  
  // 使用 b|abcdefg|f 这种边界填充模式
  xmap[-1] = 1 ; xmap[imageSize.width] = imageSize.width - 2 ;
  for( int index = 0 ; index < imageSize.width ; ++ index )
    xmap[index] = index ;
  ymap[-1] = 1 ; ymap[imageSize.height] = imageSize.height - 2 ;
  for( int index = 0 ; index < imageSize.height ; ++ index )
    ymap[index] = index ;
  
  int width = imageSize.width ;
  // 连续空间记录计算的dx dy 然后通过连续空间进行幅值与幅角的计算
  float * dbuf = new float[width*4] ;
  cv::Mat Dx(1,width,CV_32F,dbuf) ;
  cv::Mat Dy(1,width,CV_32F,dbuf+width) ;
  cv::Mat Mag(1,width,CV_32F,dbuf+width*2) ;
  cv::Mat Angle(1,width,CV_32F,dbuf+width*3) ;

  // 计算出刻度 进行角度的分类
  float angleScale = (float)(nbins/CV_PI) ; 
  
  for( int index_rows = 0 ; index_rows < imageSize.height ; ++ index_rows )
    {
      const uchar* imgPtr = (const uchar*)(Image->imageData + Image->widthStep * ymap[index_rows]);
      const uchar* prevPtr = (const uchar*)(Image->imageData + Image->widthStep * ymap[index_rows-1]);
      const uchar* nextPtr = (const uchar*)(Image->imageData + Image->widthStep * ymap[index_rows+1]);
      float* gradPtr = (float*)grad.ptr(index_rows);
      uchar* qanglePtr = (uchar*)qangle.ptr(index_rows);
      // 单通道
      if( cn == 1 )
	{
	  for( int index_cols = 0 ; index_cols < imageSize.width ; ++ index_cols )
	    {
	      dbuf[index_cols] = (float)(lut[imgPtr[xmap[index_cols+1]]]-lut[imgPtr[xmap[index_cols-1]]]) ;
	      dbuf[index_cols+width] = (float)(lut[nextPtr[index_cols]]-lut[prevPtr[index_cols]]) ;
	    }
	}
      else
	{
	  // 多通道，通过计算每个通道的梯度，选取最大的梯度来计算HOG特征向量
	  for( int index_cols = 0 ; index_cols < imageSize.width ; ++ index_cols )
	    {
	      float dx0, dy0, dx, dy, mag0, mag;
	      // 0 通道
	      dx0 = (float)(lut[imgPtr[xmap[index_cols+1]*3]] - lut[imgPtr[xmap[index_cols-1]*3]]) ;
	      dy0 = (float)(lut[nextPtr[index_cols*3]]-lut[prevPtr[index_cols*3]]) ;
	      mag0 = dx0 * dx0 + dy0 * dy0 ;
	      
	      // 1 通道
	      dx = (float)(lut[imgPtr[xmap[index_cols+1]*3+1]]-lut[imgPtr[xmap[index_cols-1]*3+1]]) ;
	      dy = (float)(lut[nextPtr[index_cols*3+1]]-lut[prevPtr[index_cols*3+1]]) ;
	      mag = dx * dx + dy * dy ;
	      
	      if( mag0 > mag )
		{
		  dx = dx0 ;
		  dy = dy0 ;
		  mag = mag ;
		}
	      
	      // 2 通道
	      dx0 = (float)(lut[imgPtr[xmap[index_cols+1]*3+2]]-lut[imgPtr[xmap[index_cols-1]*3+2]]) ;
	      dy0 = (float)(lut[nextPtr[index_cols*3+2]]-lut[prevPtr[index_cols*3+2]]) ;
	      mag0 = dx0 * dx0 + dy0 * dy0 ;
	      
	      if( mag0 > mag )
		{
		  dx = dx0 ;
		  dy = dy0 ;
		  mag = mag0 ;
		}
	      dbuf[index_cols] = dx ;
	      dbuf[width + index_cols] = dy ;
	    }
	}
      // angle 0-2*PI
      cartToPolar(Dx,Dy,Mag,Angle,false) ;
      //      for( int index = 0 ; index < imageSize.width ; ++ index )
      //	std::cout << "dx:" << dbuf[index] << "dy:" << dbuf[index+width] << "mag:" << dbuf[index+2*width] << "angle:" << dbuf[3*width+index] <<std::endl ;
      
      for( int index_cols = 0 ; index_cols < imageSize.width ; ++ index_cols )
	{
	  float mag = dbuf[width*2+index_cols] ;
	  float angle = dbuf[width*3+index_cols]*angleScale ;
	  if( angle >=9.f ) angle -= 9.f ;
	  if( angle == 9.f ) angle = 0.f ;
	  // 判断 属于那个bins
	  int hidx = cvFloor(angle) ;
	  angle -= hidx ; // 得到小数部分 作为权重
	  if( hidx < 0 ) // -5~-1 -> 4~8
	    hidx += nbins ;
	  qanglePtr[index_cols*2] = (uchar)(hidx) ;
	  ++hidx ;
	  // 如果hidx == 8 那么其相邻的那一bins就为0
	  if( hidx == nbins ) hidx = 0 ;
	  qanglePtr[index_cols*2+1] = hidx ;
	  // angle作为相邻两个bins的权值
	  gradPtr[index_cols*2] = mag*(1.f-angle) ;
	  gradPtr[index_cols*2+1] = mag*angle ;
	}
    }

  delete []dbuf ;
  delete []mapbuff ;
  return true ;
}
